#include "MyKalmanFilter.h"
#include <iostream>

namespace hitcrt {

/**
 * @brief 初始化滤波器不变的参数
 * @param param 参数列表
 * - param[0]: 6x6 Eigen::MatrixXd, 过程噪声协方差矩阵 Q
 */
void MyKalmanFilter::init(const std::vector<boost::any> &param) {
    if (param.size() != 1) {
        throw KF::paramError("MyKalmanFilter::init: 需要1个参数 (过程噪声矩阵Q)");
    }
    if (param[0].type() != typeid(Eigen::MatrixXd)) {
        throw KF::paramError("MyKalmanFilter::init: 参数类型应为 Eigen::MatrixXd");
    }

    m_processNoiseCov = boost::any_cast<Eigen::MatrixXd>(param[0]);
    if (m_processNoiseCov.rows() != m_stateDimension || m_processNoiseCov.cols() != m_stateDimension) {
        throw KF::paramError("MyKalmanFilter::init: 过程噪声矩阵Q的维度应为 6x6");
    }

    // 测量矩阵 H (3x6)，将6D状态映射到3D测量
    m_measurementMatrix = Eigen::MatrixXd::Zero(m_measureDimension, m_stateDimension);
    m_measurementMatrix.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // H = [I_3x3, 0_3x3]
}

/**
 * @brief 设置滤波器在每次迭代中可能改变的参数
 * @param param 参数列表
 * - param[0]: double, 采样时间间隔 dt
 * - param[1]: std::vector<double> (size=3), 3D测量向量 [x, y, z]
 * - param[2]: 3x3 Eigen::MatrixXd, 测量噪声协方差矩阵 R
 */
void MyKalmanFilter::setParam(const std::vector<boost::any> &param) {
    if (param.size() != 3) {
        throw KF::paramError("MyKalmanFilter::setParam: 需要3个参数 (dt, measurement, R)");
    }
    if (param[0].type() != typeid(double)) {
        throw KF::paramError("MyKalmanFilter::setParam: param[0] (dt) 类型应为 double");
    }
    if (param[1].type() != typeid(std::vector<double>)) {
        throw KF::paramError("MyKalmanFilter::setParam: param[1] (measurement) 类型应为 std::vector<double>");
    }
    auto measurementVec = boost::any_cast<std::vector<double>>(param[1]);
    if (measurementVec.size() != m_measureDimension) {
        throw KF::paramError("MyKalmanFilter::setParam: 测量向量维度应为 3");
    }
    if (param[2].type() != typeid(Eigen::MatrixXd)) {
        throw KF::paramError("MyKalmanFilter::setParam: param[2] (R) 类型应为 Eigen::MatrixXd");
    }
    auto measurementNoiseCov = boost::any_cast<Eigen::MatrixXd>(param[2]);
     if (measurementNoiseCov.rows() != m_measureDimension || measurementNoiseCov.cols() != m_measureDimension) {
        throw KF::paramError("MyKalmanFilter::setParam: 测量噪声矩阵R的维度应为 3x3");
    }

    // 更新参数
    m_deltaTime = boost::any_cast<double>(param[0]);
    for (int i = 0; i < m_measureDimension; ++i) {
        m_measurement(i, 0) = measurementVec[i];
    }
    m_measurementNoiseCov = measurementNoiseCov;

    // 更新状态转移矩阵 F (6x6)
    m_transitionMatrix = Eigen::MatrixXd::Identity(m_stateDimension, m_stateDimension);
    m_transitionMatrix.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity() * m_deltaTime;
}

/**
 * @brief 使用首次测量值初始化状态向量
 */
MyKalmanFilter& MyKalmanFilter::setStateByMeasure() {
    // 位置部分使用测量值，速度部分初始化为0
    m_statePost << m_measurement(0, 0),
                   m_measurement(1, 0),
                   m_measurement(2, 0),
                   0,
                   0,
                   0;
    return *this;
}

/**
 * @brief 重置后验误差协方差矩阵 P
 */
MyKalmanFilter& MyKalmanFilter::resetCovPost() {
    m_errorCovPost = Eigen::MatrixXd::Identity(m_stateDimension, m_stateDimension);
    // 位置的初始不确定性设置为测量的噪声水平
    m_errorCovPost.block<3, 3>(0, 0) = m_measurementNoiseCov;
    // 速度的初始不确定性设置得很大，因为我们完全不知道初始速度
    m_errorCovPost.block<3, 3>(3, 3) *= 1000.0;
    return *this;
}

} // namespace hitcrt